package zpp_nape.constraint;
import zpp_nape.Config;
import zpp_nape.Const;
import zpp_nape.util.Flags;
import DummyNapeMain;
import zpp_nape.util.Math;
import zpp_nape.util.Names;
import zpp_nape.util.Pool;
import zpp_nape.util.Lists;
import zpp_nape.shape.Circle;
import zpp_nape.shape.Edge;
import zpp_nape.space.Space;
import zpp_nape.shape.Polygon;
import zpp_nape.shape.Shape;
import zpp_nape.phys.FeatureMix;
import zpp_nape.phys.FluidProperties;
import zpp_nape.phys.Material;
import zpp_nape.phys.Body;
import zpp_nape.geom.AABB;
import zpp_nape.geom.Distance;
import zpp_nape.geom.Mat23;
import zpp_nape.geom.Vec2;
import zpp_nape.geom.VecMath;
import zpp_nape.geom.Collide;
import zpp_nape.dynamics.Arbiter;
import zpp_nape.dynamics.Contact;
import zpp_nape.dynamics.InteractionFilter;
import zpp_nape.constraint.Constraint;
import zpp_nape.dynamics.SpaceArbiterList;
import zpp_nape.constraint.Matrices;
import zpp_nape.constraint.WeldJoint;
import nape.util.Flags;
import nape.util.Lists;
import nape.util.Debug;
import nape.space.Space;
import nape.shape.Circle;
import nape.shape.Polygon;
import nape.shape.Edge;
import nape.shape.Shape;
import nape.phys.FluidProperties;
import nape.phys.Material;
import nape.phys.Body;
import nape.geom.AABB;
import nape.geom.Geom;
import nape.geom.Mat23;
import nape.geom.Vec2;
import nape.dynamics.Contact;
import nape.dynamics.Arbiter;
import nape.dynamics.InteractionFilter;
#if swc@:keep#end class ZPP_PivotJoint extends ZPP_Constraint{
    public var b1:ZPP_Body;
    public var a1localx:Float;
    public var a1localy:Float;
    public var a1relx:Float;
    public var a1rely:Float;
    public var b2:ZPP_Body;
    public var a2localx:Float;
    public var a2localy:Float;
    public var a2relx:Float;
    public var a2rely:Float;
    public var kMassa:Float;
    public var kMassb:Float;
    public var kMassc:Float;
    public var jAccx:Float;
    public var jAccy:Float;
    public var jMax:Float;
    public var frequency:Float;
    public var damping:Float;
    public var gamma:Float;
    public var biasx:Float;
    public var biasy:Float;
    public function new(){
        super();
        {
            jAccx=0;
            jAccy=0;
            {
                #if NAPE_ASSERT var res={
                    !({
                        #if flash9 var ret:Bool=untyped __global__["isNaN"](jAccx);
                        ret;
                        #else Math.isNaN(jAccx);
                        #end
                    });
                };
                if(!res)throw "assert("+"!assert_isNaN(jAccx)"+") :: "+("vec_set(in n: "+"jAcc"+",in x: "+"0"+",in y: "+"0"+")");
                #end
            };
            {
                #if NAPE_ASSERT var res={
                    !({
                        #if flash9 var ret:Bool=untyped __global__["isNaN"](jAccy);
                        ret;
                        #else Math.isNaN(jAccy);
                        #end
                    });
                };
                if(!res)throw "assert("+"!assert_isNaN(jAccy)"+") :: "+("vec_set(in n: "+"jAcc"+",in x: "+"0"+",in y: "+"0"+")");
                #end
            };
        };
        frequency=damping=0;
        jMax=ZPP_Const.POSINF;
    }
    public override function preStep(dt:Float){
        {
            a1relx=(b1.axisy*a1localx-b1.axisx*a1localy);
            a1rely=(a1localx*b1.axisx+a1localy*b1.axisy);
        };
        {
            a2relx=(b2.axisy*a2localx-b2.axisx*a2localy);
            a2rely=(a2localx*b2.axisx+a2localy*b2.axisy);
        };
        {
            var msum=b1.smass+b2.smass;
            kMassa=kMassc=msum;
            kMassb=0;
            if(b1.sinertia!=0){
                kMassa+=a1rely*a1rely*b1.sinertia;
                kMassb-=a1relx*a1rely*b1.sinertia;
                kMassc+=a1relx*a1relx*b1.sinertia;
            }
            if(b2.sinertia!=0){
                kMassa+=a2rely*a2rely*b2.sinertia;
                kMassb-=a2relx*a2rely*b2.sinertia;
                kMassc+=a2relx*a2relx*b2.sinertia;
            }
        };
        {
            var det=(kMassa*kMassc-kMassb*kMassb);
            if(det<ZPP_Const.EPSILON){
                kMassa=0;
                kMassb=0;
                kMassc=0;
            };
            else{
                det=1/det;
                var t=kMassc*det;
                kMassc=kMassa*det;
                kMassa=t;
                kMassb*=-det;
            }
        };
        if(frequency!=0){
            var biasCoef;
            {
                var X=({
                    var omega=2*Math.PI*frequency;
                    gamma=1/(omega*dt*(2*damping+omega*dt));
                    var ig=1/(1+gamma);
                    biasCoef=omega*omega*gamma;
                    gamma*=ig;
                    ig;
                });
                kMassa*=X;
                kMassb*=X;
                kMassc*=X;
            };
            {
                biasx=(b2.posx+a2relx)-(b1.posx+a1relx);
                biasy=(b2.posy+a2rely)-(b1.posy+a1rely);
                if(false){
                    biasx+=b2.bias_velx-b1.bias_velx;
                    biasy+=b2.bias_vely-b1.bias_vely;
                }
            };
            {
                var t=(-dt*biasCoef);
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                            ret;
                            #else Math.isNaN(t);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_muleq(in a: "+"bias"+",in s: "+"-dt*biasCoef"+")");
                    #end
                };
                biasx*=t;
                biasy*=t;
            };
        }
        else{
            {
                biasx=0;
                biasy=0;
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](biasx);
                            ret;
                            #else Math.isNaN(biasx);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(biasx)"+") :: "+("vec_set(in n: "+"bias"+",in x: "+"0"+",in y: "+"0"+")");
                    #end
                };
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](biasy);
                            ret;
                            #else Math.isNaN(biasy);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(biasy)"+") :: "+("vec_set(in n: "+"bias"+",in x: "+"0"+",in y: "+"0"+")");
                    #end
                };
            };
            gamma=0;
        }
        {
            {
                var t=(b1.imass);
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                            ret;
                            #else Math.isNaN(t);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_subeq(in a: "+"b1.vel"+",in b: "+"jAcc"+",in s: "+"b1.imass"+")");
                    #end
                };
                b1.velx-=jAccx*t;
                b1.vely-=jAccy*t;
            };
            {
                var t=(b2.imass);
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                            ret;
                            #else Math.isNaN(t);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_addeq(in a: "+"b2.vel"+",in b: "+"jAcc"+",in s: "+"b2.imass"+")");
                    #end
                };
                b2.velx+=jAccx*t;
                b2.vely+=jAccy*t;
            };
            b1.angvel-=(jAccy*a1relx-jAccx*a1rely)*b1.iinertia;
            b2.angvel+=(jAccy*a2relx-jAccx*a2rely)*b2.iinertia;
        };
        return false;
    }
    public override function applyImpulse(){
        var Ex:Float;
        var Ey:Float;
        {
            Ex=(b2.velx-a2rely*b2.angvel)-(b1.velx-a1rely*b1.angvel);
            Ey=(b2.vely+a2relx*b2.angvel)-(b1.vely+a1relx*b1.angvel);
        };
        var Jx:Float;
        var Jy:Float;
        {
            Jx=biasx-Ex;
            Jy=biasy-Ey;
        };
        {
            var t=kMassa*Jx+kMassb*Jy;
            Jy=kMassb*Jx+kMassc*Jy;
            Jx=t;
        };
        {
            var t=(gamma);
            {
                #if NAPE_ASSERT var res={
                    !({
                        #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                        ret;
                        #else Math.isNaN(t);
                        #end
                    });
                };
                if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_subeq(in a: "+"J"+",in b: "+"jAcc"+",in s: "+"gamma"+")");
                #end
            };
            Jx-=jAccx*t;
            Jy-=jAccy*t;
        };
        {
            var jOldx:Float;
            var jOldy:Float;
            {
                jOldx=jAccx;
                jOldy=jAccy;
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](jOldx);
                            ret;
                            #else Math.isNaN(jOldx);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(jOldx)"+") :: "+("vec_set(in n: "+"jOld"+",in x: "+"jAccx"+",in y: "+"jAccy"+")");
                    #end
                };
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](jOldy);
                            ret;
                            #else Math.isNaN(jOldy);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(jOldy)"+") :: "+("vec_set(in n: "+"jOld"+",in x: "+"jAccx"+",in y: "+"jAccy"+")");
                    #end
                };
            };
            {
                var t=(1.0);
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                            ret;
                            #else Math.isNaN(t);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_addeq(in a: "+"jAcc"+",in b: "+"J"+",in s: "+"1.0"+")");
                    #end
                };
                jAccx+=Jx*t;
                jAccy+=Jy*t;
            };
            {
                var t=(jMax);
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                            ret;
                            #else Math.isNaN(t);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_clamp(in n: "+"jAcc"+", in m: "+"jMax"+")");
                    #end
                };
                var ls=(jAccx*jAccx+jAccy*jAccy);
                if(ls>t*t){
                    {
                        #if NAPE_ASSERT var res={
                            ls!=0.0;
                        };
                        if(!res)throw "assert("+"ls!=0.0"+") :: "+("vec_clamp(in n: "+"jAcc"+", in m: "+"jMax"+")");
                        #end
                    };
                    {
                        var t=(t*ZPP_Math.invsqrt(ls));
                        {
                            #if NAPE_ASSERT var res={
                                !({
                                    #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                                    ret;
                                    #else Math.isNaN(t);
                                    #end
                                });
                            };
                            if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_muleq(in a: "+"jAcc"+",in s: "+"t*ZPP_Math.invsqrt(ls)"+")");
                            #end
                        };
                        jAccx*=t;
                        jAccy*=t;
                    };
                }
            };
            {
                Jx=jAccx-jOldx;
                Jy=jAccy-jOldy;
            };
        };
        {
            {
                var t=(b1.imass);
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                            ret;
                            #else Math.isNaN(t);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_subeq(in a: "+"b1.vel"+",in b: "+"J"+",in s: "+"b1.imass"+")");
                    #end
                };
                b1.velx-=Jx*t;
                b1.vely-=Jy*t;
            };
            {
                var t=(b2.imass);
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                            ret;
                            #else Math.isNaN(t);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_addeq(in a: "+"b2.vel"+",in b: "+"J"+",in s: "+"b2.imass"+")");
                    #end
                };
                b2.velx+=Jx*t;
                b2.vely+=Jy*t;
            };
            b1.angvel-=(Jy*a1relx-Jx*a1rely)*b1.iinertia;
            b2.angvel+=(Jy*a2relx-Jx*a2rely)*b2.iinertia;
        };
        if(frequency==0){
            var r1x:Float;
            var r1y:Float;
            {
                var ax=Math.sin(b1.rot+b1.bias_angvel);
                var ay=Math.cos(b1.rot+b1.bias_angvel);
                {
                    r1x=(ay*a1localx-ax*a1localy);
                    r1y=(a1localx*ax+a1localy*ay);
                };
            };
            var r2x:Float;
            var r2y:Float;
            {
                var ax=Math.sin(b2.rot+b2.bias_angvel);
                var ay=Math.cos(b2.rot+b2.bias_angvel);
                {
                    r2x=(ay*a2localx-ax*a2localy);
                    r2y=(a2localx*ax+a2localy*ay);
                };
            };
            {
                Ex=(b2.posx+r2x)-(b1.posx+r1x);
                Ey=(b2.posy+r2y)-(b1.posy+r1y);
                if(true){
                    Ex+=b2.bias_velx-b1.bias_velx;
                    Ey+=b2.bias_vely-b1.bias_vely;
                }
            };
            if((Ex*Ex+Ey*Ey)>6){
                var k=b1.smass+b2.smass;
                if(k>ZPP_Const.EPSILON){
                    k=0.75/k;
                    {
                        Jx=-Ex*k;
                        Jy=-Ey*k;
                        {
                            #if NAPE_ASSERT var res={
                                !({
                                    #if flash9 var ret:Bool=untyped __global__["isNaN"](Jx);
                                    ret;
                                    #else Math.isNaN(Jx);
                                    #end
                                });
                            };
                            if(!res)throw "assert("+"!assert_isNaN(Jx)"+") :: "+("vec_set(in n: "+"J"+",in x: "+"-Ex*k"+",in y: "+"-Ey*k"+")");
                            #end
                        };
                        {
                            #if NAPE_ASSERT var res={
                                !({
                                    #if flash9 var ret:Bool=untyped __global__["isNaN"](Jy);
                                    ret;
                                    #else Math.isNaN(Jy);
                                    #end
                                });
                            };
                            if(!res)throw "assert("+"!assert_isNaN(Jy)"+") :: "+("vec_set(in n: "+"J"+",in x: "+"-Ex*k"+",in y: "+"-Ey*k"+")");
                            #end
                        };
                    };
                    {
                        var t=(b1.imass);
                        {
                            #if NAPE_ASSERT var res={
                                !({
                                    #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                                    ret;
                                    #else Math.isNaN(t);
                                    #end
                                });
                            };
                            if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_subeq(in a: "+"b1.bias_vel"+",in b: "+"J"+",in s: "+"b1.imass"+")");
                            #end
                        };
                        b1.bias_velx-=Jx*t;
                        b1.bias_vely-=Jy*t;
                    };
                    {
                        var t=(b2.imass);
                        {
                            #if NAPE_ASSERT var res={
                                !({
                                    #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                                    ret;
                                    #else Math.isNaN(t);
                                    #end
                                });
                            };
                            if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_addeq(in a: "+"b2.bias_vel"+",in b: "+"J"+",in s: "+"b2.imass"+")");
                            #end
                        };
                        b2.bias_velx+=Jx*t;
                        b2.bias_vely+=Jy*t;
                    };
                    {
                        Ex=(b2.posx+r2x)-(b1.posx+r1x);
                        Ey=(b2.posy+r2y)-(b1.posy+r1y);
                        if(true){
                            Ex+=b2.bias_velx-b1.bias_velx;
                            Ey+=b2.bias_vely-b1.bias_vely;
                        }
                    };
                }
            }
            var Ka:Float;
            var Kb:Float;
            var Kc:Float;
            {
                var msum=b1.smass+b2.smass;
                Ka=Kc=msum;
                Kb=0;
                if(b1.sinertia!=0){
                    Ka+=r1y*r1y*b1.sinertia;
                    Kb-=r1x*r1y*b1.sinertia;
                    Kc+=r1x*r1x*b1.sinertia;
                }
                if(b2.sinertia!=0){
                    Ka+=r2y*r2y*b2.sinertia;
                    Kb-=r2x*r2y*b2.sinertia;
                    Kc+=r2x*r2x*b2.sinertia;
                }
            };
            {
                Jx=-Ex;
                Jy=-Ey;
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](Jx);
                            ret;
                            #else Math.isNaN(Jx);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(Jx)"+") :: "+("vec_set(in n: "+"J"+",in x: "+"-Ex"+",in y: "+"-Ey"+")");
                    #end
                };
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](Jy);
                            ret;
                            #else Math.isNaN(Jy);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(Jy)"+") :: "+("vec_set(in n: "+"J"+",in x: "+"-Ex"+",in y: "+"-Ey"+")");
                    #end
                };
            };
            {
                var det=(Ka*Kc-Kb*Kb);
                if(det<ZPP_Const.EPSILON){
                    Jx=0;
                    Jy=0;
                    {
                        #if NAPE_ASSERT var res={
                            !({
                                #if flash9 var ret:Bool=untyped __global__["isNaN"](Jx);
                                ret;
                                #else Math.isNaN(Jx);
                                #end
                            });
                        };
                        if(!res)throw "assert("+"!assert_isNaN(Jx)"+") :: "+("vec_set(in n: "+"J"+",in x: "+"0"+",in y: "+"0"+")");
                        #end
                    };
                    {
                        #if NAPE_ASSERT var res={
                            !({
                                #if flash9 var ret:Bool=untyped __global__["isNaN"](Jy);
                                ret;
                                #else Math.isNaN(Jy);
                                #end
                            });
                        };
                        if(!res)throw "assert("+"!assert_isNaN(Jy)"+") :: "+("vec_set(in n: "+"J"+",in x: "+"0"+",in y: "+"0"+")");
                        #end
                    };
                };
                else{
                    det=1/det;
                    var t=det*(Kc*Jx-Kb*Jy);
                    Jy=det*(Ka*Jy-Kb*Jx);
                    Jx=t;
                }
            };
            {
                {
                    var t=(b1.imass);
                    {
                        #if NAPE_ASSERT var res={
                            !({
                                #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                                ret;
                                #else Math.isNaN(t);
                                #end
                            });
                        };
                        if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_subeq(in a: "+"b1.bias_vel"+",in b: "+"J"+",in s: "+"b1.imass"+")");
                        #end
                    };
                    b1.bias_velx-=Jx*t;
                    b1.bias_vely-=Jy*t;
                };
                {
                    var t=(b2.imass);
                    {
                        #if NAPE_ASSERT var res={
                            !({
                                #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                                ret;
                                #else Math.isNaN(t);
                                #end
                            });
                        };
                        if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_addeq(in a: "+"b2.bias_vel"+",in b: "+"J"+",in s: "+"b2.imass"+")");
                        #end
                    };
                    b2.bias_velx+=Jx*t;
                    b2.bias_vely+=Jy*t;
                };
                b1.bias_angvel-=(Jy*r1x-Jx*r1y)*b1.iinertia;
                b2.bias_angvel+=(Jy*r2x-Jx*r2y)*b2.iinertia;
            };
        }
        return false;
    }
}
